#ifndef LB_QUATERNIONS_H_
#define LB_QUATERNIONS_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdlib.h>
typedef struct Quaternions{

double v[4];

}Quaternions;

static inline void quaternions_init(Quaternions* q )
{
    memset(q->v,0,sizeof(double)*4);
}

Quaternions quaternions_mult(Quaternions q1,Quaternions q2);

Quaternions q_rota(Quaternions q1,double* x); 

Quaternions quaternions_plus(Quaternions q1,Quaternions q2);




Quaternions quaternions_conjugate(Quaternions q );




double quaternions_norm(Quaternions q );



Quaternions quaternions_inverse(Quaternions q );

static inline double* get_matrix_from_rotation(double theta,double* axis)
{
    normalize(axis,3); 
    double *re=(double*)malloc(sizeof(double)*9);
    double sint=sin(theta);
    double cost=cos(theta);
    re[0]=cost+(1-cost)*axis[0]*axis[0];re[1]=(1-cost)*axis[0]*axis[1]-axis[2]*sint;re[2]=(1-cost)*axis[0]*axis[2]+axis[1]*sint;
    
    re[3]=sint*axis[2]+(1-cost)*axis[0]*axis[1];re[4]=cost+(1-cost)*axis[1]*axis[1];re[5]=(1-cost)*axis[1]*axis[2]-axis[0]*sint;

    re[6]=(1-cost)*axis[0]*axis[2]-axis[1]*sint;re[7]=(1-cost)*axis[2]*axis[1]+axis[0]*sint;re[8]=(1-cost)*axis[2]*axis[2]+cost;
    
    return re;
}

double* get_rotation_from_matrix(double* matrix);




double*  get_rotation_from_quaternions( Quaternions q  );

double*  get_matrix_from_quaternions( Quaternions q  );
// Quaternions get_quaternions_from_rotation(double theta,double *axis)
// {
// }



#ifdef __cplusplus
}
#endif

#endif 